//------------------------------------------------------------------------------
//  <copyright file="KukaLBR4PlusOperations.cpp" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
//  </copyright>
//------------------------------------------------------------------------------	

#include "stdafx.h"
#include "KukaLBR4ControllerCommon.h"
#include "KukaLBR4Operations.h"
#include "KukaLBR4Tracing.h"
#include <KukaLBR4Definitions.h>
#include <algorithm>
#include <math.h>
#include <stdlib.h>
#include <KukaLbr4Operations.Manifest.h>
#include <iostream>

using namespace KukaLBR4ControllerCommonNS;

namespace KukaLBR4OperationsNS
{
	/// <summary>
	/// Adds two scalar values.
	/// ppParameters[0] = ppParameters[1] + ppParameters[2]
	/// <summary>
	/// <param name="ppParameters">
	/// Array of pointers to parameters for operation.
	/// parameters[0]:  Length 1, destination for operation result;
	/// parameters[1]:  Length 1.
	/// parameters[2]:  Length 1.
	/// </param>
	KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT void CeeOpAdd(double **ppParameters)
	{
		*(ppParameters[0]) = *(ppParameters[1]) + *(ppParameters[2]);
	}

	/// <summary>
	/// Adds two vectors of length 6
	/// ppParameters[0] = ppParameters[1] + ppParameters[2]
	/// <summary>
	/// <param name="ppParameters">
	/// Array of pointers to parameters for operation.
	/// ppParameters[0]:  Length 6, destination for operation result;
	/// ppParameters[1]:  Length 6.
	/// ppParameters[2]:  Length 6.
	/// </param>
	KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT void CeeOpAddVec6(double **ppParameters)
	{
		int i;

		for(i = 0; i<6; i++)
		{
			*(ppParameters[0] + i) = *(ppParameters[1] + i) + *(ppParameters[2] + i);
		}
	}

	/// <summary>
	/// Multiplies two scalar values.
	/// ppParameters[0] = ppParameters[1] * ppParameters[2]
	/// <summary>
	/// <param name="ppParameters">
	/// Array of pointers to parameters for operation.
	/// ppParameters[0]:  Length 1, destination for operation result;
	/// ppParameters[1]:  Length 1.
	/// ppParameters[2]:  Length 1.
	/// </param>
	KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT void CeeOpMultiply(double **ppParameters)
	{
		*(ppParameters[0]) = (*(ppParameters[1])) * (*(ppParameters[2]));
	}

	/// <summary>
	/// Sets parameters[0] to 1 if the vector magnitude of parameter[1] is greater than parameter[2], otherwise
	/// sets parameter[0] to 0.
	/// <summary>
	/// <param name="ppParameters">
	/// Array of pointers to parameters for operation.
	/// ppParameters[0]:  Length 1, destination for operation result;
	/// ppParameters[1]:  Length 6.
	/// ppParameters[2]:  Length 1.
	/// </param>
	KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT void CeeOpIsMagnitudeGreaterThanVec6(double **ppParameters)
	{
		double sum = 0;
		int i;

		for (i = 0; i< 6; i++)
		{
			sum += (*(ppParameters[1] + i)) * (*(ppParameters[1] + i));
		}

		if (sqrt((sum)) > *(ppParameters[2]))
		{
			*ppParameters[0] = 1;
		}
		else
		{
			*ppParameters[0] = 0;
		}
	}

	/// <summary>
	/// Sets parameters[0] to 1 if any element of parameter[1] is greater than the corresponding
	/// element of parameter[2], otherwise sets parameter[0] to 0.
	/// <summary>
	/// <param name="ppParameters">
	/// Array of pointers to parameters for operation.
	/// ppParameters[0]:  Length 1, destination for operation result;
	/// ppParameters[1]:  Length 6.
	/// ppParameters[2]:  Length 6.
	/// </param>
	KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT void CeeOpIsAnyElementGreaterThanVec6(double **ppParameters)
	{
		for(int i = 0; i < 6; i++)
		{
			if(ppParameters[1][i] >  ppParameters[2][i])
			{
				*ppParameters[0] = 1;
				return;
			}
		}

		*ppParameters[0] = 0;
	}

	/// <summary>
	/// Copies a vector of length 7 to another vector of length 7
	/// parameters[0] = parameters[1]
	/// <summary>
	/// <param name="parameters">
	/// Array of pointers to parameters for operation.
	/// parameters[0]:  Length 7, destination for operation result;
	/// parameters[1]:  Length 7.
	/// </param>
	KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT void CeeOpCopyVec7(double **parameters)
	{
		std::copy(parameters[1], parameters[1] + 7, parameters[0]);
	}

	/// <summary>
	/// Copes a vector of length 6 to another vector of length 6
	/// parameters[0] = parameters[1]
	/// <summary>
	/// <param name="parameters">
	/// Array of pointers to parameters for operation.
	/// parameters[0]:  Length 6, destination for operation result;
	/// parameters[1]:  Length 6.
	/// </param>
	KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT void CeeOpCopyVec6(double **parameters)
	{
		std::copy(parameters[1], parameters[1] + 6, parameters[0]);
	}

	/// <summary>
	/// Sets parameter[0] to the next Cartesian goal given current value, max linear and rotational speed, and cycle rate (in ms)
	/// </summary>
	/// <param name="parameters">
	/// Array of pointers to parameters for operation.
	/// parameters[0]:  Length 12, next frame.
	/// parameters[1]:  Length 12, current frame
	/// parameters[2]:  Length 12, goal frame.
	/// parameters[3]:  Length 1, max linear velocity in m/s
	/// parameters[4]:  Length 1, max rotational velocity in rad/s
	/// parameters[5]:  Length 1, cycle time in ms
	/// </param>
	KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT void CeeOpSimpleCartesianInterpolation(double **parameters)
	{
		const double orientationDeadbandInRadians = 0.001;

		double *ppTempParameters[7];
		double pNextPosition[3];

		// X,Y,Z position in a row-major frame is elements 3, 7, and 11, repectively
		double pCurrentPosition[] = {parameters[1][3], parameters[1][7], parameters[1][11]};
		double pDesiredPosition[] = {parameters[2][3], parameters[2][7], parameters[2][11]};

		// set parameters to calculate linear component
		ppTempParameters[0] = pNextPosition;
		ppTempParameters[1] = pCurrentPosition;
		ppTempParameters[2] = pDesiredPosition;
		ppTempParameters[3] = parameters[3];
		ppTempParameters[4] = parameters[5];

		// doing one step interpolation so set 'duration in cycles' and 'current cycle' both to 1.
		double durationInCycles[1] = {1};
		double currentCycle[1] = {1};
		ppTempParameters[5] = durationInCycles;
		ppTempParameters[6] = currentCycle;

		// calculate linear component -- we'll place these into the return location below
		// after calculating the rotational component
		SimpleInterpolation(ppTempParameters, 3);

		// on to the rotational.  Utilize spherical linear interpolation, implemented with quaternions 
		// as described in:  http://dl.acm.org/citation.cfm?id=325165.325242 (Shoemake, 1985).
		double pCurrentQuat[4];
		double pGoalQuat[4];
		double pNextQuat[4];
		double pNextQuatNorm[4];

		FrameToQuaternion(parameters[1], pCurrentQuat);
		FrameToQuaternion(parameters[2], pGoalQuat);

		double quatDotProduct = QuaternionDotProduct(pCurrentQuat, pGoalQuat);

		// a negative dot product indicates we'll going around the "long" way
		// we can fix this by swapping the sign of the goal quaternion (q and -q
		// represent the same rotation, but will cause the slerp opertion to 
		// move in different directions to reach the goal location).
		if(quatDotProduct < 0)
		{
			pGoalQuat[0] = -pGoalQuat[0];
			pGoalQuat[1] = -pGoalQuat[1];
			pGoalQuat[2] = -pGoalQuat[2];
			pGoalQuat[3] = -pGoalQuat[3];

			quatDotProduct = QuaternionDotProduct(pCurrentQuat, pGoalQuat);
		}

		// make sure we don't have an invalid dot product due to numerical issues.
		if(quatDotProduct > 1)
		{
			quatDotProduct = 1;
		}

		double theta = acos(quatDotProduct);
		bool useDeadBand = true;

		// once we're withing some threshold of goal orientation, peg setpoint to goal to prevent chattering
		if(abs(theta) > orientationDeadbandInRadians)
		{
			double maxAngleDelta = parameters[4][0] * parameters[5][0] / 1000.0;

			double u = min(1, abs(maxAngleDelta/theta));

			double pCurrentQuatScaled[4];
			double pGoalQuatScaled[4];

			double sinTheta = sin(theta);

			QuaternionScalarProduct(sin((1.0-u)*theta)/sinTheta, pCurrentQuat, pCurrentQuatScaled);
			QuaternionScalarProduct(sin(u*theta)/sinTheta, pGoalQuat, pGoalQuatScaled);
			QuaternionSum(pCurrentQuatScaled, pGoalQuatScaled, pNextQuat);

			if (true == QuaternionNormalize(pNextQuat, pNextQuatNorm))
			{
				QuaternionToFrame(pNextQuatNorm, parameters[0]);
				useDeadBand = false;
			}
		}

		if (true == useDeadBand)
		{
			// create a deadband around orientation goal
			// just copy current rot matrix into result
			std::copy(parameters[1], parameters[1] + 12, parameters[0]);
		}

		// set the return value for position
		parameters[0][3] = pNextPosition[0];
		parameters[0][7] = pNextPosition[1];
		parameters[0][11] = pNextPosition[2];

		if(KukaLBR4Tracing::IsControllerEventEnabled(&SimpleCartesianInterpolationTraceEvent))
		{
			EVENT_DATA_DESCRIPTOR SimpleCartesianInterpolationDataDescriptor[6];

			// parameters are logged in order
			// first 3 parameters are of size 12 (frames)
			for(int i = 0; i < 3; ++i)
			{
				EventDataDescCreate(&SimpleCartesianInterpolationDataDescriptor[i], parameters[i], 12*sizeof(double));
			}

			// next 3 are scalars
			for(int i = 3; i < _ARRAYSIZE(SimpleCartesianInterpolationDataDescriptor); ++i)
			{
				EventDataDescCreate(&SimpleCartesianInterpolationDataDescriptor[i], parameters[i], sizeof(double));
			}

			KukaLBR4Tracing::WriteKukaLBR4ControllerEvent(&SimpleCartesianInterpolationTraceEvent,
				_ARRAYSIZE(SimpleCartesianInterpolationDataDescriptor),
				SimpleCartesianInterpolationDataDescriptor);
		}
	}

	/// <summary>
	/// Sets parameter[0] to the next joint goal given current value, max rotational speed, and cycle rate (in ms)
	/// </summary>
	/// <param name="parameters">
	/// Array of pointers to parameters for operation.
	/// parameters[0]:  Length 7, next value.
	/// parameters[1]:  Length 7, current value.
	/// parameters[2]:  Length 7, desired value.
	/// parameters[3]:  Length 1, max joint velocity in radians/s
	/// parameters[4]:  Length 1, cycle time in ms
	/// parameters[5]:  Length 1, duration of motion in cycles
	/// parameters[6]:  Length 1, current cycle number
	/// </param>
	KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT void CeeOpSimpleJointInterpolation(double **parameters)
	{
		SimpleInterpolation(parameters, NUM_JOINTS);
	}

	/// <summary>
	/// Adds two vectors of length 7
	/// parameters[0] = parameters[1] + parameters[2]
	/// <summary>
	/// <param name="parameters">
	/// Array of pointers to parameters for operation.
	/// parameters[0]:  Length 7, destination for operation result;
	/// parameters[1]:  Length 7.
	/// parameters[2]:  Length 7.
	/// </param>
	KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT void CeeOpAddVec7(double **parameters)
	{
		int i;

		for(i = 0; i<7; i++)
		{
			*(parameters[0] + i) = *(parameters[1] + i) + *(parameters[2] + i);
		}
	}

	/// <summary>
	/// Subtracts two vectors of length 7
	/// parameters[0] = parameters[1] - parameters[2]
	/// <summary>
	/// <param name="parameters">
	/// Array of pointers to parameters for operation.
	/// parameters[0]:  Length 7, destination for operation result;
	/// parameters[1]:  Length 7.
	/// parameters[2]:  Length 7.
	/// </param>
	KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT void CeeOpSubtractVec7(double **parameters)
	{
		int i;

		for(i = 0; i<7; i++)
		{
			*(parameters[0] + i) = *(parameters[1] + i) - *(parameters[2] + i);
		}
	}

	/// <summary>
	/// Sets parameter[0] to 1 if parameter[1] == parameter[2].  Otherwise sets parameter[0] to 0.
	/// </summary>
	/// <param name="parameters">
	/// Array of pointers to parameters for operation.
	/// parameters[0]:  Length 1, destination for operation result;
	/// parameters[1]:  Length 1.
	/// parameters[2]:  Length 1.
	/// </param>
	KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT void CeeOpAreScalarsEqual(double **parameters)
	{
		if (*(parameters[1]) == *(parameters[2]))
		{
			*(parameters[0]) = 1;
		}
		else
		{
			*(parameters[0]) = 0;
		}
	}

	/// <summary>
	/// Sets parameter[0] to the next goal given current value, max speed, and cycle rate (in ms)
	/// </summary>
	/// <param name="parameters">
	/// Array of pointers to parameters for operation.
	/// parameters[0]:  Length numDim, next step command.
	/// parameters[1]:  Length numDim, current value.
	/// parameters[2]:  Length numDim, desired value.
	/// parameters[3]:  Length 1, max velocity in units/s
	/// parameters[4]:  Length 1, cycle time in ms
	/// parameters[5]:  Length 1, duration of motion in cycles
	/// parameters[6]:  Length 1, current cycle number
	/// </param>
	/// <param name="numDim"> Number of dimensions to interpolate. </param>
	KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT void SimpleInterpolation(double **parameters, const int numDim)
	{
		const int nextPositionRegister = 0;
		const int startingPositionRegister = 1;
		const int destinationPositionRegister = 2;
		const int maxVelocityRegister = 3;
		const int cycleTimeInMsRegister = 4;
		const int durationInCyclesRegister = 5;
		const int currentCycleNumberRegister = 6;

		int i;
		double maxDelta = 0;
		double cycleRatio = min(1.0, parameters[currentCycleNumberRegister][0]/parameters[durationInCyclesRegister][0]);

		for(i = 0; i<numDim; ++i)
		{
			parameters[nextPositionRegister][i] = cycleRatio*(parameters[destinationPositionRegister][i] - parameters[startingPositionRegister][i]);
			if(abs(parameters[nextPositionRegister][i]) > maxDelta)
			{
				maxDelta = abs(parameters[nextPositionRegister][i]);
			}
		}

		double maxPermissibleDelta = parameters[maxVelocityRegister][0] * ((parameters[currentCycleNumberRegister][0] * parameters[cycleTimeInMsRegister][0])/1000.0);

		double ratio = 1;
		
		if(maxDelta > maxPermissibleDelta)
		{
			ratio = maxPermissibleDelta/maxDelta;

			for(i = 0; i < numDim; ++i)
			{
				parameters[nextPositionRegister][i] *= ratio;
			}
		}

		for(i = 0; i < numDim; i++)
		{
			parameters[nextPositionRegister][i] += parameters[startingPositionRegister][i];
		}

		// Logging specifically for 7-DOF LBR4 (manifest assumes fixed size of 7 for joint value descriptors)
		// Make sure we're not accidentlly logging during (for example) Cartesian interpolation.
		if(KukaLBR4Tracing::IsControllerEventEnabled(&JointInterpolationEvent) && numDim == NUM_JOINTS)
		{
			EVENT_DATA_DESCRIPTOR jointInterpolationDataDescriptor[NUM_JOINTS];

			// parameters are logged in order
			// first 3 parameters are of size 7 (joint values)
			for(int i = 0; i < 3; ++i)
			{
				EventDataDescCreate(&jointInterpolationDataDescriptor[i], parameters[i], NUM_JOINTS * sizeof(double));
			}

			// next 4 are scalars
			for(int i = 3; i < _ARRAYSIZE(jointInterpolationDataDescriptor); ++i)
			{
				EventDataDescCreate(&jointInterpolationDataDescriptor[i], parameters[i], sizeof(double));
			}

			KukaLBR4Tracing::WriteKukaLBR4ControllerEvent(&JointInterpolationEvent,
				_ARRAYSIZE(jointInterpolationDataDescriptor),
				jointInterpolationDataDescriptor);
		}
	}

	/// Copies a vector of length 12 to another vector of length 12 (length of a frame vecotr) 
	/// parameters[0] = parameters[1]
	/// <summary>
	/// <param name="parameters">
	/// Array of pointers to parameters for operation.
	/// parameters[0]:  Length 12, destination for operation result;
	/// parameters[1]:  Length 12.
	/// </param>
	KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT void CeeOpCopyFrame(double **parameters)
	{
		std::copy(parameters[1], parameters[1] + 12, parameters[0]);
	}

	/// <summary>
	/// Multiplies two frames represented as row-major arrays.
	/// parameters[0] = parameters[1] * parameters[2]
	/// <summary>
	/// <param name="parameters">
	/// Array of pointers to parameters for operation.
	/// parameters[0]:  Length 12, destination for operation result;
	/// parameters[1]:  Length 12, frame for LHS of mulitplication
	/// parameters[2]:  Length 12, frame for RHS of multiplication
	/// </param>
	KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT void CeeOpMultiplyFrames(double **parameters)
	{
		parameters[0][0] = parameters[1][0]*parameters[2][0] + parameters[1][1]*parameters[2][4] + parameters[1][2]*parameters[2][8];
		parameters[0][1] = parameters[1][0]*parameters[2][1] + parameters[1][1]*parameters[2][5] + parameters[1][2]*parameters[2][9];
		parameters[0][2] = parameters[1][0]*parameters[2][2] + parameters[1][1]*parameters[2][6] + parameters[1][2]*parameters[2][10];
		parameters[0][3] = parameters[1][0]*parameters[2][3] + parameters[1][1]*parameters[2][7] + parameters[1][2]*parameters[2][11] + parameters[1][3];

		parameters[0][4] = parameters[1][4]*parameters[2][0] + parameters[1][5]*parameters[2][4] + parameters[1][6]*parameters[2][8];
		parameters[0][5] = parameters[1][4]*parameters[2][1] + parameters[1][5]*parameters[2][5] + parameters[1][6]*parameters[2][9];
		parameters[0][6] = parameters[1][4]*parameters[2][2] + parameters[1][5]*parameters[2][6] + parameters[1][6]*parameters[2][10];
		parameters[0][7] = parameters[1][4]*parameters[2][3] + parameters[1][5]*parameters[2][7] + parameters[1][6]*parameters[2][11] + parameters[1][7];

		parameters[0][8] = parameters[1][8]*parameters[2][0] + parameters[1][9]*parameters[2][4] + parameters[1][10]*parameters[2][8];
		parameters[0][9] = parameters[1][8]*parameters[2][1] + parameters[1][9]*parameters[2][5] + parameters[1][10]*parameters[2][9];
		parameters[0][10] = parameters[1][8]*parameters[2][2] + parameters[1][9]*parameters[2][6] + parameters[1][10]*parameters[2][10];
		parameters[0][11] = parameters[1][8]*parameters[2][3] + parameters[1][9]*parameters[2][7] + parameters[1][10]*parameters[2][11] + parameters[1][11];
	}

	/// <summary>
	/// Takes a force-torque (6-dim) vector as supplied by LBR and returns 1 if the magnitude 
	/// of the 3-vector given by the first 3 elements (the X-Y-Z forces at the end point) is
	/// greater than the provided scalar.  
	/// <summary>
	/// <param name="parameters">
	/// Array of pointers to parameters for operation.
	/// parameters[0]:  Length 1. Will be 1 if force magnitude is greater than supplied threshold, zero otherwise.
	/// parameters[1]:  Length 6. [X, Y, Z, A, B, C] endpoint force-torques as given by Kuka LBR.
	/// parameters[2]:  Length 1. Scalar threshold for comparison to force magnitude.
	KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT void CeeOpIsForceMagnitudeGreaterThanScalar(double **parameters)
	{
		// compute the magnitude of the force
		double mag = sqrt(parameters[1][0]*parameters[1][0] + parameters[1][1]*parameters[1][1] + parameters[1][2]*parameters[1][2]);

		if(mag > parameters[2][0])
		{
			parameters[0][0] = 1;
		}
		else
		{
			parameters[0][0] = 0;
		}
	}

	/// <summary> Get the projection of a 3-vector onto a plane.  Plane is defined by unit vector normal to the plane.  </summary>
	/// <param name="parameters">
	/// parameters[0]:  Length 1.  Result.  Will be one if the force in parameters[1] projected into the plane orthogonal to 
	///								parameters[2] is greater than the threshold given in parameter[3].
	/// parameters[1]:  Length 3.  The Cartesian force vector (X, Y, Z).
	/// parameters[2]:  Length 3.  The unit vector normal to the plane to project the force in prameters[1] onto.
	/// parameters[3]:  Length 1.  Force magnitude threshold.
	/// </param>
	KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT void CeeIsForceInPlaneGreaterThanScalar(double **parameters)
	{
		double projectedForce[3];

		Vector3PlaneProject(parameters[1], parameters[2], projectedForce);

		double magnitude;

		VectorMagnitude(projectedForce, 3, &magnitude);

		if(magnitude > parameters[3][0])
		{
			parameters[0][0] = 1;
		}
		else
		{
			parameters[0][0] = 0;
		}
	}

	/// <summary> Get the rejection of a 3-vector from a plane.  Plane is defined by unit vector normal to the plane.  </summary>
	/// <param name="parameters">
	/// parameters[0]:  Length 1.  Result.  Will be one if the force in parameters[1] rejected from the plane orthogonal to 
	///								parameters[2] is greater than the threshold given in parameter[3].
	/// parameters[1]:  Length 3.  The Cartesian force vector (X, Y, Z).
	/// parameters[2]:  Length 3.  The unit vector normal to the plane to reject the force in prameters[1] from.
	/// parameters[3]:  Length 1.  Force magnitude threshold.
	/// </param>
	KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT void CeeIsForceOrthogonalToPlaneGreaterThanScalar(double **parameters)
	{
		// rejection from a plane is equivalent to projection onto a vector orthoganal to the plane
		// do scalar projection as we only care about the magnitude of the projection in this case.
		double magnitude;

		DotProduct(parameters[1], parameters[2], 3, &magnitude);

		if(magnitude > parameters[3][0])
		{
			parameters[0][0] = 1;
		}
		else
		{
			parameters[0][0] = 0;
		}
	}

	/// <summary>
	/// Move the arm to the "candle" position
	/// </summary>
	/// <param name="parameters">
	/// Array of pointers to parameters for operation.
	/// parameters[0]:  Length numDim, next step command.
	/// parameters[1]:  Length numDim, current value.
	/// parameters[2]:  Length 1, desired joint speed in units/s
	/// parameters[3]:  Length 1, cycle time in ms
	/// </param>
	/// <param name="numDim"> Number of dimensions to interpolate. </param>
	KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT void CeeGetJointAnglesForMotionToCandleAtConstantRate(double **parameters)
	{
		const int nextPositionRegister = 0;
		const int currentPositionRegister = 1;
		const int desiredSpeedRegister = 2;
		const int cycleTimeInMsRegister = 3;

		int i;
		if(parameters[desiredSpeedRegister][0] < 0)
		{
			throw std::exception("Move to candle speed cannot be < 0");
		}

		double delta = parameters[desiredSpeedRegister][0] * parameters[cycleTimeInMsRegister][0] / 1000.0;

		for(i = 0; i < NUM_JOINTS; ++i)
		{
			if(abs(parameters[currentPositionRegister][i]) < delta)
			{
				parameters[nextPositionRegister][i] = 0;
			}
			else if(parameters[currentPositionRegister][i] < 0)
			{
				parameters[nextPositionRegister][i] = parameters[currentPositionRegister][i] + delta;
			}
			else
			{
				parameters[nextPositionRegister][i] = parameters[currentPositionRegister][i] - delta;
			}
		}
	}

	/// <summary> Convert the rotational portion of a frame descriptor to a quaternion 
	/// There are a number of ways to do the conversion This current choice is numerically stable
	/// over a range of situations see http://en.wikipedia.org/wiki/Rotation_matrix#Quaternion
	/// </summary>
	/// <param name="pFrame"> A 3x4 frame descriptor in row-major order </param>
	/// <param name="pQuat"> 
	/// Pointer to location to store the resulting quaternion (4 dimensional).
	/// Quaternion is stored as [W, X, Y, Z], where W is the scalar component.
	/// </param>
	KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT void FrameToQuaternion(double *pFrame, double *pQuat)
	{
		const double wThreshold = 0.001;
		double r = 1.0 + pFrame[0] + pFrame[5] + pFrame[10];
		r = sqrt(__max(0, r));
		pQuat[0] = 0.5 * r;

		if (pQuat[0] > wThreshold)
		{
			// Most frequent case
			double v = 1 + pFrame[0] - pFrame[5] - pFrame[10];
			pQuat[1] = 0.5 * Sign(pFrame[9] - pFrame[6]) * sqrt(__max(0.0, v));
			v = 1 - pFrame[0] + pFrame[5] - pFrame[10];
			pQuat[2] = 0.5 * Sign(pFrame[2] - pFrame[8]) * sqrt(__max(0.0, v));
			v = 1 - pFrame[0] - pFrame[5] + pFrame[10];
			pQuat[3] = 0.5 * Sign(pFrame[4] - pFrame[1]) * sqrt(__max(0.0, v));
		}
		else 
		{
			// Deals with case when off diagonal differences are small. 
			// Most frequent treatment above can lead to unstable (inconsistent) sign behaviour
			// in the X,Y,Z components. While sign flips are ok the unstable behavior is undesirable
			double rxx = 1.0 + pFrame[0] - pFrame[5] - pFrame[10];
			double ryy = 1.0 + pFrame[5] - pFrame[0] - pFrame[10];
			double rzz = 1.0 + pFrame[10] - pFrame[0] - pFrame[5];

			if (rxx >= ryy && rxx >= rzz)
			{
				r = sqrt(__max(0, rxx));
				double s = 0.5 / r;
				pQuat[0] = (pFrame[9] - pFrame[6]) * s;
				pQuat[1] = 0.5 * r;
				pQuat[2] = (pFrame[1] + pFrame[4]) * s;
				pQuat[3] = (pFrame[8] + pFrame[2]) * s;
			}
			else if (ryy >= rxx && ryy >= rzz)
			{
				r = sqrt(__max(0, ryy));
				double s = 0.5 / r;
				pQuat[0] = (pFrame[2] - pFrame[8]) * s;
				pQuat[1] = (pFrame[1] + pFrame[4]) * s;
				pQuat[2] = 0.5 * r;
				pQuat[3] = (pFrame[9] + pFrame[6]) * s;
			}
			else
			{
				r = sqrt(__max(0, rzz));
				double s = 0.5 / r;
				pQuat[0] = (pFrame[4] - pFrame[1]) * s;
				pQuat[1] = (pFrame[2] + pFrame[8]) * s;
				pQuat[2] = (pFrame[6] + pFrame[9]) * s;
				pQuat[3] = 0.5 * r;
			}
		}
	}

	/// <summary> Returns -1 if sign of parameter is negative, 0 otherwise </summary>
	/// <param name="value"> Value to get the sign of. </param>
	/// <returns> -1 if value < 0, +1 otherwise. </returns>
	KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT double Sign(double value)
	{
		if(value < 0)
		{
			return -1.0;
		}
		else
		{
			return 1.0;
		}
	}

	/// <summary>Normalize a quaternion to the unit quaternion</summary>
	/// <param name="pQuatIn">Quaternion of form [W, X, Y, Z] to normalize</param>
	/// <param name="pQuatNorm>Normalized output quaternion in form [W, X, Y, Z]</param>
	/// <returns>false if quaternion cannot be normalized (all zeroes)</returns>
	KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT bool QuaternionNormalize(double *pQuatIn, double *pQuatNorm)
	{
		double length = QuaternionDotProduct(pQuatIn, pQuatIn);
		bool ret = false;

		if (length > 0.0)
		{
			length = sqrt(length);
			pQuatNorm[0] = pQuatIn[0] / length;
			pQuatNorm[1] = pQuatIn[1] / length;
			pQuatNorm[2] = pQuatIn[2] / length;
			pQuatNorm[3] = pQuatIn[3] / length;

			ret = true;
		}

		return ret;
	}

	/// <summary> Convert a quaternion to the rotational component of a frame description.  </summary>
	/// <param name="pQuat"> An unit quaternion in the form of [W, X, Y, Z] (W is scalar component). </param>
	/// <param name="pFrame>
	/// Pointer to location to store the resulting rotational component of the frame in row-major order.
	/// Entries correpsonding to position will not be modified.
	/// </param>
	KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT void QuaternionToFrame(double *pQuat, double *pFrame)
	{
		pFrame[0] = (pQuat[0] * pQuat[0]) + (pQuat[1] * pQuat[1]) - (pQuat[2] * pQuat[2]) - (pQuat[3] * pQuat[3]);
		pFrame[1] = 2.0 * ((pQuat[1] * pQuat[2]) - (pQuat[0] * pQuat[3]));
		pFrame[2] = 2.0 * ((pQuat[1] * pQuat[3]) + (pQuat[0] * pQuat[2]));

		pFrame[4] = 2.0 * ((pQuat[1] * pQuat[2]) + (pQuat[0] * pQuat[3]));
		pFrame[5] = (pQuat[0] * pQuat[0]) - (pQuat[1] * pQuat[1]) + (pQuat[2] * pQuat[2]) - (pQuat[3] * pQuat[3]);
		pFrame[6] = 2.0 * ((pQuat[2] * pQuat[3]) - (pQuat[0] * pQuat[1]));

		pFrame[8] = 2.0 * ((pQuat[1] * pQuat[3]) - (pQuat[0] * pQuat[2]));
		pFrame[9] = 2.0 * ((pQuat[2] * pQuat[3]) + (pQuat[0] * pQuat[1]));
		pFrame[10] = (pQuat[0] * pQuat[0]) - (pQuat[1] * pQuat[1]) - (pQuat[2] * pQuat[2]) + (pQuat[3] * pQuat[3]);
	}

	/// <summary> Returns the dot product of two quaternions. </summary>
	/// <param name="pQuat1"> An unit quaternion in the form of [W, X, Y, Z] (W is scalar component). </param>
	/// <param name="pQuat2"> An unit quaternion in the form of [W, X, Y, Z] (W is scalar component). </param>
	/// <returns> The dot product of the two quaternions. </returns>
	KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT double QuaternionDotProduct(double *pQuat1, double *pQuat2)
	{
		return pQuat1[0]*pQuat2[0] +  pQuat1[1]*pQuat2[1] + pQuat1[2]*pQuat2[2] + pQuat1[3]*pQuat2[3]; 
	}

	/// <summary> Gets the sum  of two quaternions. </summary>
	/// <param name="pQuat1"> An unit quaternion in the form of [W, X, Y, Z] (W is scalar component). </param>
	/// <param name="pQuat2"> An unit quaternion in the form of [W, X, Y, Z] (W is scalar component). </param>
	/// <param name="pQuatResult"> Location where sum will be placed in the form of [W, X, Y, Z] (W is scalar component). </param>
	KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT void QuaternionSum(double *pQuat1, double *pQuat2, double *pQuatResult)
	{
		for(int i = 0; i < 4; ++i)
		{
			pQuatResult[i] = pQuat1[i] + pQuat2[i];
		}
	}

	/// <summary> Gets the product of a scalar and a quaternion. </summary>
	/// <param name="scalar"> A scalar value. </param>
	/// <param name="pQuat"> An quaternion in the form of [W, X, Y, Z] (W is scalar component). </param>
	/// <param name="pQuatResult"> Location where product will be placed in the form of [W, X, Y, Z] (W is scalar component). </param>
	KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT void QuaternionScalarProduct(double scalar, double *pQuat, double *pQuatResult)
	{
		for(int i = 0; i < 4; ++i)
		{
			pQuatResult[i] = pQuat[i] * scalar;
		}
	}

	/// <summary> Computes the vector dot product of A and B. </summary>
	/// <param name="pA"> Vector A. </param>
	/// <param name="pB"> Vector B. </param>
	/// <param name="length"> Length of vectors A and B. </param>
	/// <param name="pResult"> Location to place the result of A dot B. </param>
	KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT void DotProduct(double *pA, double *pB, int length, double *pResult)
	{
		*pResult = 0;

		for (int i = 0; i < length; ++i)
		{
			*pResult += pA[i] * pB[i];
		}
	}

	/// <summary> Gets the cross product of pVec1 and pVec2, placing the result in pResult. </summary>
	/// <param name="pVec1"> A 3-vector. </param>
	/// <param name="pVec2"> A 3-vector. </param>
	/// <param name="pResult"> Location to place the result of the cross product of pVec1 and pVec2. </param>
	KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT void Vec3CrossProduct(double *pVec1, double *pVec2, double *pResult)
	{
		pResult[0] = pVec1[1]*pVec2[2] - pVec1[2]*pVec2[1];
		pResult[1] = pVec1[2]*pVec2[0] - pVec1[0]*pVec2[2];
		pResult[2] = pVec1[0]*pVec2[1] - pVec1[1]*pVec2[0];
	}

	/// <summary> Gets the magnitude of a vector. </summary>
	/// <param name="pVec"> Vector to get the magnitude of. </param>
	/// <param name="length"> Number of elements in the vector. </param>
	/// <param name="pResult"> Location to place the mangnitude result. </param>
	KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT void VectorMagnitude(double *pVec, int length, double *pResult)
	{
		*pResult = 0;

		DotProduct(pVec, pVec, length, pResult);

		*pResult = sqrt(*pResult);
	}

	/// <summary> Get the projection of one 3-vector onto another. </summary>
	/// <param name="pVec1">  The vector to be projected. </param>
	/// <param name="pVec2">  The vector to project pVec1 onto.  Must be a unit vector. </param>
	/// <param name="pResult">  The result of projecting pVec1 onto pVec2. </param>
	KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT void Vector3Vector3Project(double *pVec1, double *pVec2, double *pResult)
	{
		double dotResult;

		DotProduct(pVec1, pVec2, 3, &dotResult);

		for (int i = 0; i < 3; ++i)
		{
			pResult[i] = dotResult * pVec2[i];
		}
	}

	/// <summary> Get the projection of a 3-vector onto a plane.  Plane is defined by unit vector normal to the plane.  </summary>
	/// <param name="pVec">  The vector to be projected onto the plane normal to pPlaneVec. </param>
	/// <param name="pPlaneVec">  The unit vector normal to the plane to project pVec onto. </param>
	/// <param name="pResult">  The result of projecting pVec onto the plane normal to pPlaneVec. </param>
	KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT void Vector3PlaneProject(double *pVec, double *pPlaneVec, double *pResult)
	{
		double cross12[3];

		Vec3CrossProduct(pVec, pPlaneVec, cross12);
		Vec3CrossProduct(pPlaneVec, cross12, pResult);
	}
}
